//
// Created by hoyin on 2021/12/6.
//
#include "opencv2/opencv.hpp"
#include "string"
#include "TypeDefinition.h"

using namespace std;
using namespace cv;
using namespace types;

#ifndef AUST_RM_VISION_2022_UNDISTORTIMAGE_H
#define AUST_RM_VISION_2022_UNDISTORTIMAGE_H

namespace udi {
	class UndistortImage {
	private:
//	相机参数
		CameraParams cameraParams{};
	public:
		explicit UndistortImage(const CameraParams cameraParams) {
			this->cameraParams = cameraParams;
		}

		Mat undistort(const Mat& origin) {
			int rows = origin.rows;
			int cols = origin.cols;
			Mat undistortedImage = Mat(rows, cols, CV_8UC1);
			if (rows != cameraParams.height || cols != cameraParams.width) {
				cerr << "Camera parameters do not templateMatch the size of image" << endl;
				return undistortedImage;
			}
//		计算去畸变以后图像的内容
			for (int v = 0; v < rows; v++) {
				for (int u = 0; u < cols; u++) {
//				按照公式,计算点(u, v)对应到畸变图像中的坐标(u_distorted, v_distorted)
					double x = (u - cameraParams.cx) / cameraParams.fx, y = (v - cameraParams.cy) / cameraParams.fy;
					double r = sqrt(x*x + y*y);
					double x_distorted = x*(1 + cameraParams.k1*r*r + cameraParams.k2*r*r*r*r) + 2*cameraParams.p1*x*y + cameraParams.p2*(r*r + 2*x*x);
					double y_distorted = y*(1 + cameraParams.k1*r*r + cameraParams.k2*r*r*r*r) + 2*cameraParams.p2*x*y + cameraParams.p2*(r*r + 2*y*y);
					double u_distorted = cameraParams.fx*x_distorted + cameraParams.cx;
					double v_distorted = cameraParams.fy*y_distorted + cameraParams.cy;

//				赋值 (最近邻插值)
					if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
						undistortedImage.at<uchar>(v, u) = origin.at<uchar>((int) v_distorted, (int) u_distorted);
					} else {
						undistortedImage.at<uchar>(v, u) = 0;
					}
				}
			}
			return undistortedImage;
		}

		static Mat getImageFromFile(const string& imageFile) {
			Mat image = imread(imageFile, 0);
			return image;
		}


		static int originMain(string imageFile) {
//		畸变参数
			double k1 = 0.287463, k2 = -0.248256, p1 = 0.010009, p2 = 0.014989;
//		内参
			double fx = 527.018275, fy = 524.474715, cx = 354.058664, cy = 241.564878;
			Mat image = imread(imageFile, 0);	//需要使用灰度图,CV_8UC1
			int rows = image.rows, cols = image.cols;
			Mat image_undistort = Mat(rows, cols, CV_8UC1);	// 去畸变以后的结果

//		计算去畸变以后图像的内容
			for (int v = 0; v < rows; v++) {
				for (int u = 0; u < cols; u++) {
//				按照公式,计算点(u, v)对应到畸变图像中的坐标(u_distorted, v_distorted)
					double x = (u - cx) / fx, y = (v - cy) / fy;
					double r = sqrt(x*x + y*y);
					double x_distorted = x*(1 + k1*r*r + k2*r*r*r*r) + 2*p1*x*y + p2*(r*r + 2*x*x);
					double y_distorted = y*(1 + k1*r*r + k2*r*r*r*r) + 2*p2*x*y + p2*(r*r + 2*y*y);
					double u_distorted = fx*x_distorted + cx;
					double v_distorted = fy*y_distorted + cy;

//				赋值 (最近邻插值)
					if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
						image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
					} else {
						image_undistort.at<uchar>(v, u) = 0;
					}
				}
			}

//		画出去畸变后的图像
			imshow("distorted", image);
			imshow("undistorted", image_undistort);
			waitKey();
			return 0;
		}
	};

}


#endif //SLAMBOOK2_UNDISTORTIMAGE_H
